#include <math.h>
#include <fstream> 
#include <string.h> 

#include "StdCalculator.h"
#include "linear.h"
#include "holeFilling.h"
#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkShiftScaleImageFilter.h"
#include "itkImportImageFilter.h"
#include "itkBinaryThresholdImageFilter.h"
#include "itkRescaleIntensityImageFilter.h"
#include "itkCastImageFilter.h"
#include "itkImageFileWriter.h"
#include "itkImageRegionIteratorWithIndex.h"
#include "itkBinaryErodeImageFilter.h"
#include "itkBinaryDilateImageFilter.h"
#include "itkBinaryBallStructuringElement.h"
#include "itkBinaryThinningImageFilter.h"
#include "itkImageDuplicator.h"
#include "itkIdentityTransform.h"
#include "itkBSplineInterpolateImageFunction.h"
#include "itkResampleImageFilter.h"

typedef unsigned short PixelType; // pixel type for Phase image. Note: unsigned char is not enough
typedef float          PixelValueType;
typedef itk::Image< PixelType, 3 >         ImageType3D; // The format of input Phase image is "3D Analyze"
typedef itk::Image< PixelType, 2 >         ImageType2D; // some temporary image variable and all the output images are 2D
typedef itk::Image< PixelValueType, 3 >    VelocityImageType; // Just for temporary image storage 3D
typedef itk::Image< PixelValueType, 2 >    ValueImageType; // Just for temporary image calculation,Not for output
// unsigned char can't be used to store phase image

void CornerPointRemoval(int rows, int columns, ImageType2D::Pointer bwThinningImg);
int LengthOfBranch(ImageType2D::IndexType startPoint, ImageType2D::Pointer bwImg);
float Distance(float pixelSpacing, ImageType2D::IndexType pointLast, ImageType2D::IndexType pointNow);
int SkeletonExtraction(float pixelSpacing, ImageType2D::IndexType endPoint, ImageType2D::Pointer aortaThinningImg, ImageType2D::IndexType * skeletonContourS, float * skeletonDistS);
int FindNextPoint(ImageType2D::IndexType point, ImageType2D::Pointer bwImg, ImageType2D::IndexType *pointNext);
void RemovePointsFromImg(int nPoints, ImageType2D::IndexType * pointList, ImageType2D::Pointer bwImg);
ImageType2D::IndexType EndPointLocation(int rows, int columns, ImageType2D::Pointer aortaThinningImg);
void get_cross_sections(int nPointsOnSkeleton, ImageType2D::IndexType * contourIdx, ImageType2D::Pointer aortaImage, int *nPointOnSections, ImageType2D::IndexType ** pointListOnSections);
int maxArray1D(int length, float * Array);
int minArray1D(int length, float * Array);
void Nearest(int nPointList, float **pointList, float velocity, int *idx);
float time_from_2point(float *pointA, float *pointB, float velocity);
float time_for_velocity(int nPointList, float **tsvCurves ,float velocity);
bool GetNeighborIndexes(bool *usefulIndicator, int nSkeletonPoints, int pIndex, int *neighborIndexes);
void AverageTD(int *neighborIndexes, float **tdPoints, float *TD);
void SeekTD(bool *avgUsefulIndicator, float **avgTDPoints, int nSkeletonPoints, int index, float *segmentEndTD);
void Smooth(int nVelocity, float * velocity, int span);
void SplineInterpolate(float timeInterval, int nVelocity, float *velocity, float newTimeInterval, float *newVelocity);
int IndexOf(int nVelocity, float * velocityCurve, float percent);
float GetArrivalTime(int nPoints, float *time, float *velocity, float vPoint);
void LineTraverseFit(int nList, float **pList, float point[2], float &slope, float &intercept);

#define NEIGHBORSIZE 10

int main( int argc, char **argv) {

	const int RescaleSlope = 2; // Constant
	const int RescaleIntercept =-4096; //Constant
	const float partitionRatio = 0.5;
	int strelSize = 3; // morphological structuring element, which will be adjusted later according to the image size, baseline: PixelSpacing =1.56 

	typedef itk::ImageFileReader<ImageType3D> ReaderType3D; // Import the Analyze format 3D-image
	ReaderType3D::Pointer reader3D = ReaderType3D::New();


	reader3D->SetFileName(argv[1]);
	int Slices=atoi(argv[2]);//<# of frames>
	int Rows=atoi(argv[3]);//<height of the images>
	int Columns=atoi(argv[4]);//<width of the images>
	int givenPoint[2];
	givenPoint[0]=atoi(argv[5]);//<xLabel >
	givenPoint[1]=Rows-atoi(argv[6]);//<yLabel, ITK take the bottom-left corner as origin >
	int upperThreshold=atoi(argv[7]);//<upper threshold, default: 80>
	int lowerThreshold=atoi(argv[8]);//<lower threshold, default: 60>
	float RepetitionTime=atof(argv[9]); //ms <RepetitionTime, default:13.1>
	float PixelSpacing=atof(argv[10]); //mm <PixelSpacing, default:1.56>
	int VENC = atoi(argv[11]); // cm/s <VENC, default:150>

	typedef itk::ImageFileWriter<ImageType2D> WriterType; // Output 2D-images, like: Correlation_Image.dcm, Correlation_Binary_Image.dcm, Segmented_Aorta.dcm, Segmented_Aorta_Thining.dcm, Aorta_Skeleton.dcm,
	WriterType::Pointer writer= WriterType::New();

	typedef itk::ShiftScaleImageFilter<ImageType3D, VelocityImageType > ShiftScaleFilterType;//Magnitude->Velocity: VENC
	ShiftScaleFilterType::Pointer shiftFilter = ShiftScaleFilterType::New(); 
	shiftFilter->SetInput( reader3D->GetOutput() );
	shiftFilter->SetScale( RescaleSlope*VENC/4096.0 );//We don't use RescaleSlope here to prevent int/int=int ~
	shiftFilter->SetShift( RescaleIntercept/RescaleSlope );
	shiftFilter->Update();

	VelocityImageType::Pointer velocityImage = shiftFilter->GetOutput();//Just for temporary 3D image storage



	// 1. Obtain the correlation image ****************************************

	// First, get velocity waveform sample (from the given point on ascending/descending aorta)
	VelocityImageType::IndexType pixelIndex3D;	
	pixelIndex3D[0] = givenPoint[0];  
	pixelIndex3D[1] = givenPoint[1]; //YLabel from bottom to top ^ 
	pixelIndex3D[2] =  0; //Non-useful, just taken as an iterative variable

	PixelValueType *velocityWaveformSample = new PixelValueType[Slices];
	for(pixelIndex3D[2]=0; pixelIndex3D[2] < Slices; pixelIndex3D[2]++)
	{
		velocityWaveformSample[pixelIndex3D[2]] = velocityImage->GetPixel(pixelIndex3D);
	}
	
	pixelIndex3D[0] = 0; // Reset for the following calculation of correlation
	pixelIndex3D[1] = 0; 
	pixelIndex3D[2] = 0; 

	
	// Correlation image Output Pipeline (buffer->imageFilter->scaleConversion->writer)

	// Second, construct a data buffer to store the calculated correlation value		
	const unsigned int  numberOfPixels =  Columns * Rows;
	PixelValueType * correlationBuffer = new PixelValueType[numberOfPixels]; //For Correlation Image Output
	PixelValueType * iCorrelationBuffer = correlationBuffer; //Keep the correlationBuffer point to beginning of buffer

	PixelValueType *iVelocityWaveform = new PixelValueType[Slices]; // Velocity Waveform at point pixelIndex3D
	StdCalculator calc; 
	for(pixelIndex3D[1] = Rows-1; pixelIndex3D[1] >= 0; pixelIndex3D[1]--) // row by row, From top to bottom
	{
		for(pixelIndex3D[0]=0; pixelIndex3D[0] < Columns; pixelIndex3D[0]++)// x
		{
			for(pixelIndex3D[2]=0; pixelIndex3D[2] < Slices; pixelIndex3D[2]++)
			{
				iVelocityWaveform[ pixelIndex3D[2] ] = velocityImage->GetPixel(pixelIndex3D);
			}
			calc.SetValues(iVelocityWaveform, velocityWaveformSample, Slices);
			*iCorrelationBuffer++ = fabs(calc.Calculate_Correlation());
		}
	}



	// Third, assign the constructed buffer to a imageFilter.
	typedef itk::ImportImageFilter< PixelValueType, 2 > ImportFilterType;//ValueImageType, this filter will be reused later
	ImportFilterType::Pointer importFilter = ImportFilterType::New();
	ImportFilterType::SizeType size;
	size[0] = Columns; // size along X- horizon direction
	size[1] = Rows; // size along Y- vertical direction
	ImportFilterType::IndexType start;
	start.Fill( 0 ); //Initialize
	ImportFilterType::RegionType region;
	region.SetIndex( start );
	region.SetSize( size );
	importFilter->SetRegion( region );
	const bool importFilterWillOwnTheBuffer = false;
	importFilter->SetImportPointer( correlationBuffer, numberOfPixels, importFilterWillOwnTheBuffer );// need update?

	// Finally, rescale the pixel intensity to normal range for image storage
	typedef itk::RescaleIntensityImageFilter<ValueImageType, ImageType2D > RescaleFilterType;
	RescaleFilterType::Pointer rescaleFilter = RescaleFilterType::New();
	rescaleFilter->SetInput( importFilter->GetOutput() );//******
	rescaleFilter->SetOutputMinimum( 0 );
	rescaleFilter->SetOutputMaximum( 255 );
	writer->SetFileName( "Correlation_Image.dcm" );
	writer->SetInput(rescaleFilter->GetOutput());
	writer->Update();



	// 2. Get binary (Black & White) Image **********************************
	// Here we can also use the itk: ValueImageType::Pointer correlationImage = importFilter->GetOutput();
	PixelValueType * binaryBuffer = new PixelValueType[ numberOfPixels ];
	PixelValueType * iBinaryBuffer = binaryBuffer; //Keep the binaryBuffer point to start address of buffer
	iCorrelationBuffer = correlationBuffer; // pointer address reset
	for(int j=0; j < Rows; j++) // row by row
	{
		for(int i=0; i < Columns; i++)// x direction
		{
			if ( j < ceil(Rows*partitionRatio) )
				*iBinaryBuffer++= (*iCorrelationBuffer++) < (upperThreshold/100.0) ? 0 : 255;
			else
				*iBinaryBuffer++= (*iCorrelationBuffer++) < (lowerThreshold/100.0) ? 0 : 255;
		}
	}

	importFilter->SetImportPointer( binaryBuffer, numberOfPixels, importFilterWillOwnTheBuffer ); // Reuse the former defined filter to assign data butter
	typedef itk::CastImageFilter<ValueImageType, ImageType2D > CastFilterType;
	CastFilterType::Pointer castFilter = CastFilterType::New();
	castFilter->SetInput( importFilter->GetOutput() );	
	ImageType2D::Pointer correlationBinaryImage = castFilter->GetOutput();// The correlation Binary image, Just for the better code readability
	writer->SetFileName( "Correlation_Binary_Image.dcm" );
	writer->SetInput (correlationBinaryImage);
	writer->Update();


	// 2.4 Fill holes

	typedef itk::BinaryFillholeImageFilter<ImageType2D> I2LType;
	I2LType::Pointer reconstruction = I2LType::New();
	reconstruction->SetInput(correlationBinaryImage);
	reconstruction->SetFullyConnected(8);
	reconstruction->SetForegroundValue(255);
	// reconstruction->SetBackgroundValue( 0 );
	writer->SetFileName("Correlation_Binary_Image_Filled.dcm");
	writer->SetInput(reconstruction->GetOutput());
	writer->Update();


	// 3. Remove background by Morphology Operation ** BinaryImage->Erode->Dilate *********************
	typedef itk::BinaryBallStructuringElement< PixelType, 2  >  StructuringElementType;
	StructuringElementType  structuringElement;
	strelSize = int( strelSize*(0.7813/PixelSpacing) + 0.5 ) ; // Adjust the size of mophological operator, int executes "rounding"
	structuringElement.SetRadius( strelSize );  // Should be adjusted
	structuringElement.CreateStructuringElement();

	typedef itk::BinaryErodeImageFilter< ImageType2D, ImageType2D,  StructuringElementType >  ErodeFilterType;
	ErodeFilterType::Pointer  binaryErode1  = ErodeFilterType::New();
	binaryErode1->SetKernel( structuringElement );

	typedef itk::BinaryDilateImageFilter< ImageType2D, ImageType2D,  StructuringElementType > DilateFilterType;
	DilateFilterType::Pointer binaryDilate1 = DilateFilterType::New();
	binaryDilate1->SetKernel( structuringElement );

//	binaryErode1->SetInput( correlationBinaryImage );
	binaryErode1->SetInput( reconstruction->GetOutput() );
	binaryDilate1->SetInput( binaryErode1->GetOutput() );
	binaryErode1->SetErodeValue( 255 );
	binaryDilate1->SetDilateValue( 255 );
	writer->SetFileName( "2Morphology1.dcm" );
	writer->SetInput (binaryDilate1->GetOutput());
	//writer->Update(); // This image is not important


/*
	// 4. Fill holes & Connect ascending and descending aorta by Morphology Operation **** ->Dilate->Erode ***************
	ErodeFilterType::Pointer  binaryErode2  = ErodeFilterType::New();
	DilateFilterType::Pointer binaryDilate2 = DilateFilterType::New();

	strelSize = int( strelSize*5.7 + 0.5 ) ; // Adjust the size of mophological operator, Rounding, used to be 1.7
	structuringElement.SetRadius( strelSize ); 
	structuringElement.CreateStructuringElement();
	binaryErode2->SetKernel(  structuringElement );
	binaryDilate2->SetKernel( structuringElement );

	binaryDilate2->SetInput( binaryDilate1->GetOutput() );
	binaryErode2->SetInput( binaryDilate2->GetOutput() );
	binaryErode2->SetErodeValue( 255 );
	binaryDilate2->SetDilateValue( 255 );
	writer->SetFileName( "Segmented_Aorta.dcm" );
	ImageType2D::Pointer aortaSegmentedImage = binaryErode2->GetOutput();// Binary segmented aorta image, Just for the better code readability 
	writer->SetInput(aortaSegmentedImage);
	writer->Update();
*/


//	ImageType2D::Pointer aortaSegmentedImage = correlationBinaryImage;
	ImageType2D::Pointer aortaSegmentedImage = binaryDilate1->GetOutput();
	// 5. Aorta Thining **********************************************************
	typedef itk::BinaryThinningImageFilter< ImageType2D, ImageType2D >  ThinningFilterType;
	ThinningFilterType::Pointer  binaryThinning  = ThinningFilterType::New();
	binaryThinning->SetInput(aortaSegmentedImage); // segmented aorta image import
	typedef itk::RescaleIntensityImageFilter<ImageType2D, ImageType2D > RescaleFilterType2; // Actually the rescale process could be ommited, try it later
	RescaleFilterType2::Pointer rescaleFilter2 = RescaleFilterType2::New();
	ImageType2D::Pointer aortaThinningImage = binaryThinning->GetOutput();// 0-1 aorta thining image, Just for the better code readability
    rescaleFilter2->SetInput( aortaThinningImage );// 0-1 ->0-255
	rescaleFilter2->SetOutputMinimum( 0 );
	rescaleFilter2->SetOutputMaximum( 255 );
	writer->SetFileName( "Segmented_Aorta_Thining.dcm" );
	writer->SetInput(rescaleFilter2->GetOutput());
	writer->Update();


	////////////////////////////// Dec
	/*
		int c,r;
		bool **bwImg = new bool*[Columns];
		for (c = 0; c < Columns; c++)
			bwImg[c] = new bool[Rows];
	*/

	std::cout << "argc == " << argc<<  std::endl;
	if (argc == 13) { // Update the aortaThinningImage
		std::cout << "Load manual fixed skeleton from file." << std::endl;

		// Load potential skeleton contour points from TXT **********************
		int nPotentialPoints; // to be loaded
		ImageType2D::IndexType * potentialPoints = new ImageType2D::IndexType[2000]; // to be loaded

		std::ifstream roughSkeleton;
		roughSkeleton.open(argv[12]);
		if (!roughSkeleton)   // Test for error.
		{
		    std::cerr << "Error opening file:\n";
		    exit(1);
		}
		roughSkeleton >> nPotentialPoints;
		for(int i=0; i < nPotentialPoints; i++)
		{
			roughSkeleton >> potentialPoints[i][0]>> potentialPoints[i][1];
//			std::cout<< "potentialPoints["<<i<<"] == "<< potentialPoints[i] << std::endl;
		}

		roughSkeleton.close();


		// initialize all pixels to zero
		ImageType2D::IndexType iPoint;
		for (iPoint[1] = 0; iPoint[1] < Columns; iPoint[1]++)
			for (iPoint[0] = 0; iPoint[0] < Rows; iPoint[0]++) {
				aortaThinningImage->SetPixel(iPoint, 0);
			}

		// load potential points
		for (int p = 0; p < nPotentialPoints; p++) {
			iPoint = potentialPoints[p];
			aortaThinningImage->SetPixel(iPoint, 1);
		}

	}
	////////////////////////////// Dec

   // Corner removal
    CornerPointRemoval(Rows, Columns, aortaThinningImage);
//	writer->SetFileName( "Aorta_Skeleton.dcm" ); //Segmented_Aorta_Thining_Corner_Point_Removed
//	writer->SetInput(aortaThinningImage);
//	writer->Update();



	// 6. Manually Connection or Spline curve fitting



    // 7. Skeleton extraction ***********************************************************

	//  endPoint location (the lowest point on descending aorta) ************************
	ImageType2D::IndexType endPoint;
	endPoint = EndPointLocation(Rows, Columns, aortaThinningImage);

	// Skeleton contour tracing
	ImageType2D::IndexType * skeletonContour = new ImageType2D::IndexType[500];// Assume <=500 points, [column, row] ----contour begin at the startPoint
	float * skeletonDistance = new float[500];// distance from nowPoint to startPoint
	int nPointsOnSkeleton = SkeletonExtraction(PixelSpacing, endPoint, aortaThinningImage, skeletonContour, skeletonDistance);// number of points on skeleton contour

	// if the extracted skeleton is less than 80 pixels, try to find the next segment
	int SegNum=0;
	std::cout << "The endPoint on Segment is: " <<endPoint<< std::endl;
	std::cout << "The number of extracted skeleton points" <<++SegNum<<" is: " << nPointsOnSkeleton<< std::endl;
	while(nPointsOnSkeleton<50)// This should be taken as a input parameter later
	{
		if(nPointsOnSkeleton==0)
		{
			std::cout << "No Skeleton Segment is available. Please check the Seed Point position!" << std::endl;
			return -1; // Image is empty
		}

		RemovePointsFromImg(nPointsOnSkeleton, skeletonContour, aortaThinningImage);
		endPoint = EndPointLocation(Rows, Columns, aortaThinningImage);
		std::cout << "The endPoint on Segment is: " <<endPoint<< std::endl;
		nPointsOnSkeleton = SkeletonExtraction(PixelSpacing, endPoint, aortaThinningImage, skeletonContour, skeletonDistance);
		std::cout << "The nPointsOnSkeleton on Segment " <<++SegNum<<" is: " << nPointsOnSkeleton<< std::endl;
	}


	// Output extracted skeleton image
	for(int i=0; i<nPointsOnSkeleton; i++)
		aortaThinningImage->SetPixel( skeletonContour[i], 2 ); 

	writer->SetFileName( "Aorta_Skeleton.dcm" );
	writer->SetInput(aortaThinningImage);
	writer->Update();

	// Output skeleton contour points to TXT **********************
	std::ofstream fSkeleton;
	fSkeleton.open( "Aorta-Skeleton.txt",std::ios::out );
	// There might be one pixel difference due to 0-starting
	for(int i=0; i < nPointsOnSkeleton; i++)
	{
		fSkeleton <<skeletonContour[i] <<std::endl;
	}
	fSkeleton.flush();
	fSkeleton.close();


	/////////////////////////////////// Oct 10
	//	ImageType2D::Pointer aortaSegmentedImage = correlationBinaryImage;
	//	ImageType2D::Pointer aortaSegmentedImage = binaryDilate1->GetOutput();
		// 5. Aorta Thining **********************************************************
	/*
		binaryThinning->SetInput(binaryDilate1->GetOutput()); // segmented aorta image import
		aortaThinningImage = binaryThinning->GetOutput();// 0-1 aorta thining image, Just for the better code readability

		rescaleFilter2->SetInput( aortaThinningImage );// 0-1 ->0-255
			rescaleFilter2->SetOutputMinimum( 0 );
			rescaleFilter2->SetOutputMaximum( 255 );
			writer->SetFileName( "Segmented_Aorta_Thining2.dcm" );
			writer->SetInput(rescaleFilter2->GetOutput());
			writer->Update();

	    CornerPointRemoval(Rows, Columns, aortaThinningImage);

		//  endPoint location (the lowest point on descending aorta) ************************
		endPoint = EndPointLocation(Rows, Columns, aortaThinningImage);

		// Skeleton contour tracing
		nPointsOnSkeleton = SkeletonExtraction(PixelSpacing, endPoint, aortaThinningImage, skeletonContour, skeletonDistance);// number of points on skeleton contour
		while(nPointsOnSkeleton<50)// This should be taken as a input parameter later
		{
			if(nPointsOnSkeleton==0)
			{
				std::cout << "No Skeleton Segment is available. Please check the Seed Point position!" << std::endl;
				return -1; // Image is empty
			}

			RemovePointsFromImg(nPointsOnSkeleton, skeletonContour, aortaThinningImage);
			endPoint = EndPointLocation(Rows, Columns, aortaThinningImage);
			std::cout << "The endPoint on Segment is: " <<endPoint<< std::endl;
			nPointsOnSkeleton = SkeletonExtraction(PixelSpacing, endPoint, aortaThinningImage, skeletonContour, skeletonDistance);
			std::cout << "The nPointsOnSkeleton on Segment " <<++SegNum<<" is: " << nPointsOnSkeleton<< std::endl;
		}*/
		/////////////////////////// Oct 10


	// Show Magnitude + Skeleton ***************************************
	//typedef itk::ImageFileReader<ImageType2D> ReaderType2D; // Import the analyze 3D-image
	//ReaderType2D::Pointer reader2D= ReaderType2D::New();
	//reader2D->SetFileName( "f://PWVC++//ns35Magnitude.IMA");
	//reader2D->Update();
	//ImageType2D::Pointer inputImage = ImageType2D::New();
	//inputImage = reader2D->GetOutput();
	//for (int i=0;i<nPointsOnSkeleton;i++) // Calculate the pulse arrival time later (When the cross_sections are extracted)
	//{
	//	inputImage->SetPixel( skeletonContourIdxS[i], 255 );
	//}
	//writer->SetFileName( "5MagnitudeskeletonContour.dcm" );
	//writer->SetInput(inputImage);
	//writer->Update();



	//%% Cross section extraction**************************************
	int *nPointOnSections = (int *)malloc(nPointsOnSkeleton * sizeof(int));//number of points on cross sections, nPointsOnSkeleton X 1
	int maxRadius = 5; //% The maximum number of points on each sections should be less than 11
	
	ImageType2D::IndexType ** pointListOnSections = (ImageType2D::IndexType **)malloc(nPointsOnSkeleton * sizeof(ImageType2D::IndexType *));// nPointsOnSkeleton X 2*(2*maxRadius+1), row col  row col ...
	for(int i=0;i<nPointsOnSkeleton;i++)
	{
		pointListOnSections[i] = new ImageType2D::IndexType[ 2*maxRadius+1 ];// [][2*maxRadius+1]
	}

	ImageType2D::Pointer rawAortaSegmentedImage = binaryDilate1->GetOutput();
//	correlationBinaryImage
//	get_cross_sections(nPointsOnSkeleton, skeletonContour, aortaSegmentedImage, //Input
	get_cross_sections(nPointsOnSkeleton, skeletonContour, rawAortaSegmentedImage, //Input
		nPointOnSections,pointListOnSections);//Output



	// Calculate the average velocity waveform with pulse arrival time marker **************************


	// float pwvPoints[nPointsOnSkeleton][2]
	float ** pwvPoints = new float*[nPointsOnSkeleton];// Used to store the points for pwvLine fitting, m/cm
	for(int i=0; i<nPointsOnSkeleton; i++) 
		pwvPoints[i] = new float[2];

	// float velocityWaveforms[nPointsOnSkeleton][Slices]
	float ** velocity = (float **)malloc(nPointsOnSkeleton * sizeof(float *));// Store the velocity waveform of points along skeleton, [ phase# velocity(cm/s)]
	for(int i=0;i<nPointsOnSkeleton;i++)
	{
		velocity[i] = new float[Slices];// [][Slices], The extracted velocity waveform of points on skeleton, default x-label is phase#
	}


    	// 2013-03-13

  int nUsefulPoints = 0; // % The number of useful points
  int nVelocityFlip = 0;
  float timeInterval = RepetitionTime;
  int nVelocity = Slices;
  float newTimeInterval = 1.0; // 1ms interval, used for interpolation
  int nNewVelocity = round(nVelocity * timeInterval / newTimeInterval);

  float * velocityInterpolated = new float[nNewVelocity];
  for (int i = 0; i < nNewVelocity; i++)
    velocityInterpolated[i] = 0;

// tdPoints = zeros(nPointsOnSkeleton,2);
  float ** tdPoints = new float*[nPointsOnSkeleton]; // Used to store the all the time-distance points [pulseArrivalTime dist2StartPoint] along the skeleton
  for (int i = 0; i < nPointsOnSkeleton; i++)
    tdPoints[i] = new float[2];


	for(int iPoint=0; iPoint<nPointsOnSkeleton; iPoint++) //% nPointsOnSkeleton = number of skeleton contour points
	{
//		std::cout << "iPoint = " << iPoint << std::endl;
		pixelIndex3D[0] = skeletonContour[iPoint][0]; //Column location of the i-th point, for temporary use
		//	pixelIndex3D[1] = skeletonContour[iPoint][1];
		pixelIndex3D[1] = Rows - skeletonContour[iPoint][1]; //Row location of the i-th point, Shoud be Rectified later when solving the up-down row problem

//	    velocity = reshape(velocityWaveformArray(iPointRow,iPointColumn,1:Slices), Slices, 1);
		for (int iSlice = 0; iSlice < Slices; iSlice++) {
			pixelIndex3D[2] = iSlice;
			velocity[iPoint][iSlice] = velocityImage->GetPixel(pixelIndex3D);
//			std::cout << "velocity[" << iSlice << "]=" << velocity[iSlice] << std::endl;
		}


	    int maxIndex = maxArray1D(Slices, velocity[iPoint]);
	    float maxValue = velocity[iPoint][maxIndex]; // Find the maximum velocity
	    int minIndex = minArray1D(Slices, velocity[iPoint]);
	    float minValue = velocity[iPoint][minIndex]; // Find the ,minimum velocity

		// Negative -> Positive Velocity
		if ((nVelocityFlip > 10) || (-minValue > maxValue)) // Reverse the velocity waveform of point which lies on the descending aorta )
		{
			nVelocityFlip = nVelocityFlip + 1;
            // velocity = -velocity;
			for (int iSlice = 0; iSlice < Slices; iSlice++)
			  velocity[iPoint][iSlice] = -velocity[iPoint][iSlice];
		}

//		std::cout <<"timeInterval="<< timeInterval <<"; nVelocity="<< nVelocity <<"; newTimeInterval="<< newTimeInterval << std::endl;
//		for (int iSlice = 0; iSlice < 10; iSlice++)
//		std::cout << "-velocity[" << iSlice << "]=" << velocity[iSlice] << std::endl;

// without smoothing
	    Smooth(nVelocity, velocity[iPoint], 3);
	    SplineInterpolate(timeInterval, nVelocity, velocity[iPoint], newTimeInterval, velocityInterpolated); // after interpolation and smoothing, Time-Shifted Velocity Curves, ms cm/sec
//		for (int i = 0; i < 10; i++)
//			std::cout << "velocityInterpolated[" << i << "]=" << velocityInterpolated[i] << std::endl;

	    int peak20Idx = IndexOf(nNewVelocity, velocityInterpolated, 0.2); // % it also indicate the time, since the interval is 1ms
	    int peak80Idx = IndexOf(nNewVelocity, velocityInterpolated, 0.8);
	    int upslopeSpan = peak80Idx-peak20Idx+1;
	    int peak50Idx = IndexOf(nNewVelocity, velocityInterpolated, 0.5); // % target velocity level
	    float halfPeakVelocity = velocityInterpolated[peak50Idx];

	    float arrivalTime;
	    if(upslopeSpan>20)
	    {
		    float * upslopeTime = new float[upslopeSpan];
			for (int i = peak20Idx; i <= peak80Idx; i++)
				upslopeTime[i-peak20Idx] = i; // since the interval is 1ms

			float * upslopeVelocity = new float[upslopeSpan];
			for (int i = peak20Idx; i <= peak80Idx; i++)
				upslopeVelocity[i-peak20Idx] = velocityInterpolated[i];

			arrivalTime = GetArrivalTime(upslopeSpan, upslopeTime, upslopeVelocity, halfPeakVelocity); //% **The pulse arrival time**
	    }
	    else
	        arrivalTime = 0;

//	    std::cout << "arrivalTime = " << arrivalTime << std::endl;

	    bool valid = 0;
	    float XCorrelation = correlationBuffer[ skeletonContour[iPoint][1]*Columns + skeletonContour[iPoint][0] ];
//	    std::cout << "XCorrelation = " << XCorrelation << std::endl;
		if ((arrivalTime > peak20Idx) && (arrivalTime < peak80Idx) && (XCorrelation > 0.7) && (upslopeSpan < 80)) //% Should be adjusted later for better results
		{
			tdPoints[nUsefulPoints][0] = arrivalTime;
			tdPoints[nUsefulPoints][1] = skeletonDistance[iPoint];
			nUsefulPoints = nUsefulPoints + 1;
			valid = 1;
//			std::cout << "tdPoints["<< nUsefulPoints <<"] = {"<< tdPoints[nUsefulPoints][0] <<", "<< tdPoints[nUsefulPoints][1] <<"}"<< std::endl;
		}
    	// 2013-03-13
	}


	// Output the Time-shifted Velocity curves*******************************


	//%% Fig. 6: PWV point scattering & fitting ************************

		// 1st time fitting (Linear regression)
		float *x = (float *)malloc(nUsefulPoints * sizeof(float));
		float *y = (float *)malloc(nUsefulPoints * sizeof(float));

		for(int i=0; i < nUsefulPoints; i++)
		{
			x[i] = tdPoints[i][0];
			y[i] = tdPoints[i][1];
		}
		Maths::Regression::Linear A(nUsefulPoints, x, y);
		std::cout << "The global PWV after the 1st calculation is " << 1000*A.getSlope() <<" cm/s"<< std::endl;

		// 2nd time fitting (by removing outliers)
		float * dist2FittingLine = new float[nUsefulPoints];// Calculate the distance from each pwvPoints to the fitting line
		for(int i=0; i < nUsefulPoints; i++)
		{
			dist2FittingLine[i] = abs(-A.getSlope()*tdPoints[i][0]+tdPoints[i][1]-A.getIntercept() ) / sqrt(A.getSlope()*A.getSlope()+1);
		}

		StdDeviation calcStd;
		calcStd.SetValues(dist2FittingLine, nUsefulPoints);
		float meanDist = calcStd.CalculateMean();
		float stdDist = calcStd.Calculate_StandardDeviation();

		int nNewUsefulPoints = 0;
		for(int iPoint=0; iPoint<nUsefulPoints; iPoint++)
			if ( dist2FittingLine[iPoint] < (meanDist+stdDist) ) // 1-std
			{				
				x[iPoint] = tdPoints[iPoint][0];
				y[iPoint] = tdPoints[iPoint][1];
				nNewUsefulPoints = nNewUsefulPoints + 1;
			}

//		std::cout << "nNewUsefulPoints = " << nNewUsefulPoints << std::endl;
		Maths::Regression::Linear B(nNewUsefulPoints, x, y);
		std::cout << "The global PWV after the 2nd calculation is " << 1000*B.getSlope() <<" cm/s"<< std::endl;


		// Output the tdpoint list used for PWV calculation to TXT *******************************
		                std::ofstream fTDPoints;
		                fTDPoints.open("Pulse-Wave-Velocity-Fitting.txt",std::ios::out);

		                // Output 2nd Fitting results
		                for(int i=0; i < nUsefulPoints; i++)
		                {
		                        if (i==nUsefulPoints-1)
		                        {
		                            fTDPoints << x[i] <<"_"<< y[i]<< std::endl;
		                        }
		                        else
		                        {
		                            fTDPoints << x[i] <<"_"<< y[i]<< "_" ;
		                        }
		                }

		                fTDPoints << "0_" << B.getIntercept() << std::endl;
		                fTDPoints << B.getSlope() << std::endl;


		                fTDPoints.flush();
		                fTDPoints.close();


		// Calculate local PWV for several fixed segments *******************************

		int nSegments = 2;
		std::cout << "The skeleton is divided into " << nSegments <<" segments."<< std::endl;

		float *pwvSegment = new float[nSegments];

		float *dPoint = new float[nNewUsefulPoints];
		for (int i = 0; i < nNewUsefulPoints; i++)
			dPoint[i] = tdPoints[i][1];

		float intercept = B.getIntercept();
		float slope = B.getSlope();
		float keyPoint[2] = {(0-intercept)/slope,  0}; // (y-b)/k
		for (int iSegment=0; iSegment<nSegments; iSegment++) // Calculate PWV for each segments respectively.
		{
			int segmentStartIdx = IndexOf(nNewUsefulPoints, dPoint, float(iSegment)/nSegments);
			int segmentEndIdx = IndexOf(nNewUsefulPoints, dPoint, float(iSegment+1)/nSegments);
			int nPointsOnSegment = segmentEndIdx-segmentStartIdx+1;
//			std::cout << "segmentStartIdx= " << segmentStartIdx << std::endl;
//			std::cout << "segmentEndIdx= " << segmentEndIdx << std::endl;

			float ** tdSegmentPoints = new float*[nPointsOnSegment];
			for(int i=0; i<nPointsOnSegment; i++)
				tdSegmentPoints[i] = new float[2];

			for (int i = segmentStartIdx; i <= segmentEndIdx; i++)
			{
				tdSegmentPoints[i-segmentStartIdx][0] = tdPoints[i][0];
				tdSegmentPoints[i-segmentStartIdx][1] = tdPoints[i][1];
			}

			LineTraverseFit(nPointsOnSegment, tdSegmentPoints, keyPoint, slope, intercept);
			pwvSegment[iSegment] = slope;
			std::cout << "The local PWV on segment "<< iSegment+1 <<" is " << pwvSegment[iSegment]*1000 <<" cm/s."<< std::endl;
		}


		// Output local PWV of fixed segments to TXT*******************************
		std::ofstream fLocalPWV;
		fLocalPWV.open("Local-Segment-PWVs.txt",std::ios::out);

		for(int iSegment=0; iSegment<nSegments; iSegment++)
		{
			int segmentStartIdx = IndexOf(nPointsOnSkeleton, skeletonDistance, float(iSegment)/nSegments);
			int segmentEndIdx = IndexOf(nPointsOnSkeleton, skeletonDistance, float(iSegment+1)/nSegments);
                                 //StratPoint   StratPoint   PWV
			fLocalPWV << skeletonContour[segmentStartIdx][0]<<" "<< skeletonContour[segmentStartIdx][1] <<" "<< skeletonContour[segmentEndIdx][0]<<" "<< skeletonContour[segmentEndIdx][1] <<" "<<skeletonDistance[segmentEndIdx]-skeletonDistance[segmentStartIdx]<<" "<< pwvSegment[iSegment]<< std::endl;
		}

		fLocalPWV.flush();
		fLocalPWV.close();



		// Output Time-shifted Velocity Curves to TXT *********************************
		std::ofstream fTshifted;
		fTshifted.open("Time-Shifted-Velocity-Curves.txt",std::ios::out);

		for(int i=0; i<nPointsOnSkeleton; i++)
		{
			if (i%10==0) //Every 10 pixel away have one curve?
			{
				for (int t = 0; t < Slices; t++)
				{
					float x = RepetitionTime*t ;//velocityWaveforms[iSection][iSlice] // Could be moved out of the for-loop
					float y = velocity[i][t];

					if (t==Slices-1)
					{
						fTshifted << x <<"_"<< y << std::endl;
					}
					else
					{
						fTshifted << x <<"_"<< y << "_" ;
					}
				}
			}
		}

		fTshifted.flush();
		fTshifted.close();

		return 0;
}




void CornerPointRemoval(int rows, int columns, ImageType2D::Pointer bwThinningImg)// I/O bwThinningImg
/* Remove the corner points, such as
   1           1             1 2 3
   X 1  and  1 X             4 i 5   */
{
	ImageType2D::IndexType iPoint,point4,point2,point5;

	for(iPoint[1]=rows-2; iPoint[1] >= 1; iPoint[1]--) // Row by row, and from bottom to top, leave the border aside
	{
		for(iPoint[0]=1; iPoint[0] < columns-1; iPoint[0]++)// x
		{   
			point2[0] = iPoint[0];
			point2[1] = iPoint[1]-1;

			point4[0] = iPoint[0]-1;
			point4[1] = iPoint[1];

			point5[0] = iPoint[0]+1;
			point5[1] = iPoint[1];

			if (   ( (bwThinningImg->GetPixel(point2)==1) && (bwThinningImg->GetPixel(point4)==1) )  ||  ( (bwThinningImg->GetPixel(point2)==1) && (bwThinningImg->GetPixel(point5)==1) )   )
				bwThinningImg->SetPixel(iPoint,0);	// Remove the corner point	
		}
	}
}


ImageType2D::IndexType EndPointLocation(int rows, int columns, ImageType2D::Pointer bwThinningImg)// Only input
/*	% Input:
            % rows                no comments
            % columns             no comments
            % aortaThinningImg    aorta thining black&white image. */
{
	ImageType2D::IndexType endPoint;

	ImageType2D::IndexType iPoint; // xlabel-->iPoint[0]; ylabel-->iPoint[1].
	bool flag4for = 0;
	for(iPoint[1]=rows-1; iPoint[1] >= 0; iPoint[1]--) // Row by row, and from bottom to top
	{
		if ( flag4for == 1 )
			break;

		for(iPoint[0]=0; iPoint[0] < columns; iPoint[0]++)// x
		{   
			if ( bwThinningImg->GetPixel(iPoint) == 1 )
			{
				endPoint[0] = iPoint[0];
				endPoint[1] = iPoint[1];
				flag4for = 1;
				break;				
			}
		}
	}

	return endPoint;
}


int SkeletonExtraction(float pixelSpacing, ImageType2D::IndexType endPoint, ImageType2D::Pointer aortaThinningImg, // Input
					   ImageType2D::IndexType * skeletonContourS, float * skeletonDistS) // Output
/*	% Input:
            % pixelSpacing        1 X 1                 mm
            % endPoint            1 X 2                 [column, row]
            % aortaThinningImg    Column X Row          Black & White aorta binary image
    % Output:
             % nPointsOnSkeleton    1 X 1                     "Return" the number of points on skeleton
             % skeletonContourS     nPointsOnSkeleton X 2     [column, row] 
			 % skeletonDistS        nPointsOnSkeleton X 1     distance from pointNow to startPoint */
{
	// Contour tracing (from endPoint to startPoint to get the location of every points on aorta skeleton contour)

	if (aortaThinningImg->GetPixel(endPoint) == 0) // Found one new point on aorta
	{
		return 0; // This is an empty image
	}

	ImageType2D::IndexType pointNow = endPoint;

	ImageType2D::IndexType skeletonContourE[500];// Assume <=500 points, [column, row]----contour begin at the endPoint
	skeletonContourE[0] = endPoint; 

	float skeletonDistE[500]; //  physical distance from pointNow to endPoint
	skeletonDistE[0] = 0;	

	int nPointsOnSkeleton = 1; // Once we get the accurate number of points on skeleton, we discard the assumption of 500 points.

	// Make a backup copy of aortaThinningImg in case it was destroyed--could be omitted
	typedef itk::ImageDuplicator< ImageType2D > DuplicatorType;
	DuplicatorType::Pointer duplicator = DuplicatorType::New();
	duplicator->SetInputImage(aortaThinningImg);
	duplicator->Update();
	ImageType2D::Pointer skeletonImg = duplicator->GetOutput();

	bool converged = false;
	while (converged==false)
	{
		RemovePointsFromImg(1, &pointNow, skeletonImg);// Remove the current point from the skeleton
		ImageType2D::IndexType pointNext[7]; // since we assume the 8-connectivity.
		int nBranches = FindNextPoint(pointNow, skeletonImg, pointNext);

		if (nBranches==0) // It's already the last point
			converged = true;

		else if (nBranches==1) // just follow the trunk
		{
			//                                                                      distance between pointLast and pointNow
			skeletonDistE[nPointsOnSkeleton] = skeletonDistE[nPointsOnSkeleton-1] + Distance(pixelSpacing, pointNow, pointNext[0]);// distance(cm) from pointNow to endPoint

			pointNow = pointNext[0];        
			skeletonContourE[nPointsOnSkeleton] = pointNow;

			nPointsOnSkeleton = nPointsOnSkeleton + 1;
		}

		else // We meet the branches
		{
			RemovePointsFromImg(nBranches, pointNext, skeletonImg);// Remove all the branch start points from the skeleton

			float *branchLength = new float[nBranches]; // in pixels

			for (int i=0; i<nBranches; i++)
				branchLength[i] = LengthOfBranch(pointNext[i], skeletonImg);// skeletonImg should be remain the same after calling this function

			int trunkIdx = maxArray1D(nBranches, branchLength);//  the longest branch is the trunk
			delete branchLength;

			//                                                              distance between pointLast and pointNow
			skeletonDistE[nPointsOnSkeleton] = skeletonDistE[nPointsOnSkeleton-1] + Distance(pixelSpacing, pointNow, pointNext[trunkIdx]); // distance(cm) from pointNow to endPoin

			pointNow = pointNext[trunkIdx];
			skeletonContourE[nPointsOnSkeleton] = pointNow;

			nPointsOnSkeleton = nPointsOnSkeleton + 1;
		}

	}

	// startPoint correction
	// skeletonContourS:--startPoint-->endPoint
	for (int i=0; i<nPointsOnSkeleton; i++) // Calculate the pulse arrival time later (When the cross_sections are extracted)
	{
		skeletonContourS[i] = skeletonContourE[nPointsOnSkeleton-1-i]; // start-> <-end
		skeletonDistS[i] = skeletonDistE[nPointsOnSkeleton-1] - skeletonDistE[nPointsOnSkeleton-1-i];
	}

	return nPointsOnSkeleton;
}


int LengthOfBranch(ImageType2D::IndexType startPoint, ImageType2D::Pointer bwImg)
/*% Actually what we are calculating is just the number of pixels
% I don't think it's necessary to calculate the real phisical length here
% Our aim is just for branch removal

% point is the start point of the branch */
{
	int length = 1;
	ImageType2D::IndexType pointNow = startPoint;

	// Make a backup copy of bwImg in case it was destroyed, Since we just measure the branch length here.
	typedef itk::ImageDuplicator< ImageType2D > DuplicatorType;
	DuplicatorType::Pointer duplicator = DuplicatorType::New();
	duplicator->SetInputImage(bwImg);
	duplicator->Update();
	ImageType2D::Pointer bwBranchImg = duplicator->GetOutput();

	bool converged = false;

	while(converged==false)
	{
		RemovePointsFromImg(1, &pointNow, bwBranchImg);// Remove the current point from the skeleton
		ImageType2D::IndexType pointNext[7]; // since we assume the 8-connectivity.
		int nBranches = FindNextPoint(pointNow, bwBranchImg, pointNext);

		if (nBranches==0) // It's already the last point
			converged = true;

		else if (nBranches==1) // just follow the trunk
		{
			pointNow = pointNext[0];        
			length = length + 1;
		}

		else // We meet the branches
		{
			float *branchLength = new float[nBranches]; // in pixels

			RemovePointsFromImg(nBranches, pointNext, bwBranchImg);// Remove all the branch start points from the skeleton

			for (int i=0; i<nBranches; i++)
				branchLength[i] = LengthOfBranch(pointNext[i], bwBranchImg);

			int trunkIdx = maxArray1D(nBranches, branchLength);//  the longest branch is the trunk

			length = length + branchLength[trunkIdx]; // int-float conversion
			converged = true; // Really?
			delete branchLength;
		}
	}

	return length;
}


void RemovePointsFromImg(int nPoints, ImageType2D::IndexType * pointList, ImageType2D::Pointer bwImg)// Only input
/* Input:
         % nPoints     Number of points to be removed
         % pointList   The list of points (row and column location) to be removed
         % bwImg       The working image */
{
	for(int i=0; i<nPoints; i++)
		bwImg->SetPixel( pointList[i], 0 ); // Remove all the branch start points from the skeleton
}


int FindNextPoint(ImageType2D::IndexType pointNow, ImageType2D::Pointer bwImg,// Input
				  ImageType2D::IndexType *pointNext)//Output
/* Input:
         % pointNow    Current point--locates at '0'
		 % bwImg       The working image
   Output:
          % nBranches   "Return" Number of branches stem from the current point
		  % pointNext   The start points of all the subsequent branches, which should be allocated before call this function
		  %             ImageType2D::IndexType pointNext[7]; // since we assume the 8-connectivity.
    %  1 2 3
    %  4 0 5
    %  6 7 8          */    
{
	int nBranches = 0;
	ImageType2D::IndexType pointTmp; // Just for tempory use

	int Rows = bwImg->GetLargestPossibleRegion().GetSize()[1];// Revised on Jan 23rd
    int Columns = bwImg->GetLargestPossibleRegion().GetSize()[0];// Revised on Jan 23rd

	for(int c=-1;c<=+1;c++) // Column
	{
		for(int r=-1;r<=+1;r++) // Row
		{
			pointTmp[0] = pointNow[0]+c; 
			pointTmp[1] = pointNow[1]+r; 

			if (pointTmp[0]>Columns || pointTmp[1]>Rows) // Revised on Jan 23rd
			{
                continue;
			} // Revised on Jan 23rd

			if (bwImg->GetPixel(pointTmp) == 1) // Found one new point on aorta
			{                    
				pointNext[nBranches] = pointTmp;
				nBranches = nBranches + 1;
			}
		}
	}

	return nBranches;
}


float Distance(float pixelSpacing, ImageType2D::IndexType pointLast, ImageType2D::IndexType pointNow)// Only input
// calculate the physical length between two adjacent points
// pixelSpacing is in mm
// len is in cm
{
	float len;
	len = 0.1*pixelSpacing*sqrt( float(abs(pointNow[0]-pointLast[0]) + abs(pointNow[1]-pointLast[1])) );
	return len;
}


void get_cross_sections(int nPointsOnSkeleton, ImageType2D::IndexType * contourIdx, ImageType2D::Pointer aortaImage,//Input
						int *nPointOnSections, ImageType2D::IndexType ** pointListOnSections)// Output
						/*    % 3-neighbour Gradient Method
						% Input:
						% nPointsOnSkeleton                     number of points on aorta skeleton contour
						% contourIdx                   point list on aorta skeleton contour, 2 X nSkeleton, row col
						% aortaImage                   segmented aorta black&white image.
						%
						% Output:
						% pointListOnSections                point list on aorta cross sections, nSkeleton X 2(2maxRadius+1), row col row col ...
						% nPointsOnSections            number of points on sections, nSkeleton X 1 */
{
	//int width = aortaImage->GetLargestPossibleRegion().GetSize()[0];
	//int heigth = aortaImage->GetLargestPossibleRegion().GetSize()[1];// % bug fixed 0930

	const int maxRadius=5; //% The maximum number of points on each sections should be less than 11

	int nSkeleton = nPointsOnSkeleton; // % number of points on skeleton
	float tangent;
	float xDist,yDist;

	ImageType2D::IndexType crossSectionIdx[2*maxRadius+1];// Along the tangent line, 10 points on each side
	for (int i=0; i<nSkeleton; i++)
	{
		if(i==0)//% Take forward differences on start and end edges
		{
			xDist = contourIdx[1][0] - contourIdx[0][0];
			yDist = contourIdx[1][1] - contourIdx[0][1];
		}
		else if(i==nSkeleton-1)//% Take forward differences on start and end edges
		{
			xDist = contourIdx[nSkeleton-1][0] - contourIdx[nSkeleton-2][0];
			yDist = contourIdx[nSkeleton-1][1] - contourIdx[nSkeleton-2][1];
		}
		else//% Take centered differences on interior points
		{
			xDist = contourIdx[i+1][0] - contourIdx[i-1][0];
			yDist = contourIdx[i+1][1] - contourIdx[i-1][1];
		}


		if(yDist == 0)//same x
			// tangent needn't to be calculated
			for( int j=0; j<2*maxRadius+1; j++)
			{
				crossSectionIdx[j][0] = contourIdx[i][0];//same x
				crossSectionIdx[j][1] = contourIdx[i][1]-maxRadius+j;
			}		
		else
		{
			tangent = -xDist/yDist;

			//	std::cout << contourIdx[i] << std::endl;
			//	std::cin>>pause;

			for( int j=0; j<2*maxRadius+1; j++)
			{
				crossSectionIdx[j][0] = contourIdx[i][0]-maxRadius+j;// Known x
				// yt = gradient*(xt - x0) + y0; %Tangent line
				crossSectionIdx[j][1] = ceil(tangent*(-maxRadius+j)) + contourIdx[i][1];// Calculate y
			}	
		}

		//% And(Cross_Section, segmentedAortaBwimg) to get the cross section point location list
		//% But does it make sense to average the pulse arrival time of all points on
		//% the same cross section?
		int nPoints = 0;
		for( int k=0; k<2*maxRadius+1; k++)
		{
			if ( (crossSectionIdx[k][0]>=0) && (crossSectionIdx[k][0]<aortaImage->GetLargestPossibleRegion().GetSize()[0])
				&& (crossSectionIdx[k][1]>=0) && (crossSectionIdx[k][1]<aortaImage->GetLargestPossibleRegion().GetSize()[1]) )// % bug fixed 0930
			{
				if (aortaImage->GetPixel(crossSectionIdx[k])==255)
				{
					pointListOnSections[i][nPoints]=crossSectionIdx[k];
					nPoints++;
				}
//				else if (nPoints > 0) //% Prevent the tangent line cross two sections of aorta
//					break;
			}// % 0930
		}
		nPointOnSections[i] = nPoints;  

	}

}


int maxArray1D(int length, float * Array)
{
	int maxIdx = 0;
	float maxValue=Array[maxIdx];
	for(int i=0;i<length;i++) 
		if(Array[i]>maxValue)
		{
			maxIdx=i; 
			maxValue=Array[maxIdx];
		}
		return  maxIdx;
}


int minArray1D(int length, float * Array)
{
	int minIdx = 0;
	float minValue=Array[minIdx];
	for(int i=0;i<length;i++) 
		if(Array[i]<minValue)
		{
			minIdx=i;
			minValue=Array[minIdx];
		}
		return  minIdx;
}


void Nearest(int nPointList, float **pointList, float velocity,// Input
			 int *idx)//Output
{
	int minIdx,maxIdx;
	minIdx=0;
	maxIdx=0;
	for (int i=1; i<nPointList; i++) // start from 2nd point to prevent minIdx =-1
	{
		if(pointList[i][1]==velocity)
		{
			minIdx = i;
			maxIdx = i; 
			break;
		}
		else if (pointList[i][1]>velocity)// Climing the mountain
		{           
			minIdx = i-1;// believe me, i can't be less than 1 if the curve is smooth enough.
			maxIdx = i;
			break;
		}
	}

	idx[0]=minIdx;
	idx[1]=maxIdx;
}


float time_from_2point(float *pointA, float *pointB, float velocity)
{
	float time4Velocity;

	if (pointB[1]==pointA[1]) // y2 == y1
		time4Velocity = (pointB[1] + pointA[1])/2.0; // (x2+x1)/2
	else 	
		time4Velocity = (velocity-pointA[1]) * (pointB[0]-pointA[0])/(pointB[1]-pointA[1]) + pointA[0];// x = (y-y1)*(x2-x1)/(y2-y1) + x1	

	return time4Velocity;
}


float GetArrivalTime(int nPoints, float *time, float *velocity, float vPoint)
{

	float tPoint;
	Maths::Regression::Linear Line(nPoints, time, velocity);

	float slope = Line.getSlope();
	float intercept = Line.getIntercept();

//% vPoint = slope*tPoint+intercept
	tPoint = (vPoint - intercept) / slope;
	return tPoint;

}


float time_for_velocity(int nPointList, float **tsvCurves ,float velocity)// import Slices*****FFDSFSDF
// tsvCurves[Slices][2] --time shifted velocity curves
// locate the time point of the corresponding velocity
{
	float time=0;
	int nearestIdx[2];
	Nearest(nPointList, tsvCurves, velocity, nearestIdx);// nearestIdx[0]--Minimum one of the nearest index; nearestIdx[1]--Maximum one of the nearest index.

	if (nearestIdx[0]==nearestIdx[1])// velocity is luck on the curve points
		time = tsvCurves[nearestIdx[0]][0];
	else
		time = time_from_2point( tsvCurves[nearestIdx[0]], tsvCurves[nearestIdx[1]], velocity);

	return time;
}


void Smooth(int nVelocity, float * velocity, int span) // velocity -> Input/Output
{
	float * vTempory = new float[nVelocity];
	for (int i = 0; i < nVelocity; i++)
		vTempory[i] = velocity[i];

	for (int i = 0; i < nVelocity; i++)
	{
		if ((i - 1 < 0) || (i + 1 >= nVelocity))
			velocity[i] = vTempory[i];
		else
			velocity[i] = (vTempory[i - 1] + vTempory[i] + vTempory[i + 1]) / span;
	}
}


int IndexOf(int nVelocity, float * velocityCurve, float percent)

//% Return the index of the proper velocity point which is close to percent*max(velocityCurve);
//% Specifically: if percent<0.5 , search from top to bottom, until meet the
//%               first point which is smaller than percent*max(velocityCurve);
//%               if percent>0.5 , search from bottom to top, until meet the
//%               first point which is larger than percent*max(velocityCurve).
//% Note: percent is the relative percent.
{
	int idx;
//	std::cout << "IndexOf: nVelocity= " << nVelocity << std::endl;
//	std::cout << "IndexOf: percent= " << percent << std::endl;

	int indexMax = maxArray1D(nVelocity, velocityCurve);
	float velocityMax = velocityCurve[indexMax]; // Find the maximum velocity
	int indexMin = minArray1D(indexMax, velocityCurve);
	float velocityMin = velocityCurve[indexMin]; // Find the ,minimum velocity

	if (percent == 0) // % for return BaseVelocity index
		idx = indexMin;
	else
	{
		float velocityTarget = percent * (velocityMax - velocityMin) + velocityMin;

		if (percent > 0.5)
			for (int i = 0; i <= indexMax; i++)
			{
				if (velocityCurve[i] >= velocityTarget)
				{
					idx = i;
					break;
				}

			}
		else
			for (int i = indexMax; i > 1; i--)
			{
				if (velocityCurve[i] <= velocityTarget)
				{
					idx = i;
					break;
				}

			}
	}

	return idx;
}

bool GetNeighborIndexes(bool *usefulIndicator, int nSkeletonPoints, int pIndex, // Input
		                int *neighborIndexes) // Output
// nSkeletonPoints                    number of points on aorta skeleton
// usefulIndicator[nSkeletonPoints]   indicate whether a skeleton point is useful or not
// pIndex                             a given point index
// neighborIndexes[NEIGHBORSIZE]      NEIGHBORSIZE closest point indexes to the given point within the 2*NEIGHBORSIZE neighborhood
{
	int count=0;
	for(int idx=0; idx<=NEIGHBORSIZE; idx++) // the center point is recorded twice, so we only take indexes from [1] to [NEIGHBORSIZE] for later use
	{
		if(pIndex-idx >= 0)
		{
			if(usefulIndicator[pIndex-idx])
			{
				neighborIndexes[count]=pIndex-idx;
				count++;
			}
		}

		if(pIndex+idx < nSkeletonPoints)
		{
			if(usefulIndicator[pIndex+idx])
			{
				neighborIndexes[count]=pIndex+idx;
				count++;
			}
		}


		if(count>NEIGHBORSIZE) // get NEIGHBORSIZE useful points already
			return true;
	}

		return false; // the number of useful points is less than NEIGHBORSIZE within the 2*NEIGHBORSIZE neighborhood
}


void AverageTD(int *neighborIndexes, float ** tdPoints, // Input
		       float *TD) // Output
{
	TD[0]=0; // avg time
	TD[1]=0; // avg distance

	for(int i=1; i<=NEIGHBORSIZE; i++)
	{
			TD[0] += tdPoints[neighborIndexes[i]][0];
			TD[1] += tdPoints[neighborIndexes[i]][1];
	}

	TD[0] = TD[0]/NEIGHBORSIZE;
	TD[1] = TD[1]/NEIGHBORSIZE;

}

void SeekTD(bool *avgUsefulIndicator, float **avgTDPoints, int nSkeletonPoints, int index, float *segmentEndTD)
{

	int count=0;
	for(int shift=0; shift<nSkeletonPoints; shift++) // the center point is recorded twice, so we only take indexes from [1] to [NEIGHBORSIZE] for later use
	{
		if(index+shift>0 && index+shift<nSkeletonPoints)
		{
			if(avgUsefulIndicator[index+shift])
			{
				segmentEndTD[0] = avgTDPoints[index+shift][0];
				segmentEndTD[1] = avgTDPoints[index+shift][1];
				return;
			}
		}


		if(index-shift>0 && index-shift<nSkeletonPoints)
		{
			if(avgUsefulIndicator[index-shift])
			{
				segmentEndTD[0] = avgTDPoints[index-shift][0];
				segmentEndTD[1] = avgTDPoints[index-shift][1];
				return;
			}
		}
	}

}

/*
int NextIndexes(int **tdPoints, int nSkeletonPoints, int pIndex, float dst)
// return the index of point which is 'dst' away to the pIndex point
{

}
*/

void SplineInterpolate(float timeInterval, int nVelocity, float *velocity, float newTimeInterval,// Input
		               float *newVelocity) // Output
// nNewVelocity = round(nVelocity*timeInterval/newTimeInterval);
// velocity -> float velocity[nVelocity]
// newVelocity -> float newVelocity[nNewVelocity]
// SplineInterpolate(13.1, 65, velocity, 1.0, velocityInterpolated);
{
	typedef float PixelType;
	const unsigned int Dimension = 1;
	typedef itk::Image<PixelType, Dimension> ImageType;

	typedef itk::ImportImageFilter<PixelType, Dimension> ImportFilterType;
	ImportFilterType::Pointer importFilter = ImportFilterType::New();

	ImportFilterType::SizeType size; // Input Parameter
	size[0] = nVelocity; // size along X
	ImportFilterType::IndexType start;
	start.Fill(0);

	ImportFilterType::RegionType region;
	region.SetIndex(start);
	region.SetSize(size);

	importFilter->SetRegion(region);

	double origin[Dimension];
	origin[0] = 0.0; // X coordinate
	importFilter->SetOrigin(origin);

	double inSpacing[Dimension]; // Input Parameter
	inSpacing[0] = timeInterval; // along X direction
	importFilter->SetSpacing(inSpacing);

	const unsigned int numberOfPixels = size[0];
	PixelType * localBuffer = velocity; // Input Parameter

//	for (int i = 0; i < 10; i++)
//		std::cout << "localBuffer[" << i << "]=" << localBuffer[i] << std::endl;
//
//	std::cout <<"SplineInterpolate-Debug: before importFilter" << std::endl;
//	const bool importImageFilterWillOwnTheBuffer = true;
	const bool importImageFilterWillOwnTheBuffer = false;
	importFilter->SetImportPointer(localBuffer, numberOfPixels, importImageFilterWillOwnTheBuffer);
	importFilter->Update();

//	std::cout <<"SplineInterpolate-Debug: after importFilter" << std::endl;

	typedef itk::IdentityTransform<double, 1> T_Transform;
	T_Transform::Pointer _pTransform = T_Transform::New();
	_pTransform->SetIdentity();

	typedef itk::BSplineInterpolateImageFunction<ImageType, double, double> T_Interpolator;
	T_Interpolator::Pointer _pInterpolator = T_Interpolator::New();
	_pInterpolator->SetSplineOrder(3);

	// The resampler type itself.
	typedef itk::ResampleImageFilter<ImageType, ImageType> T_ResampleFilter;

	// Instantiate the resampler. Wire in the transform and the interpolator.
	T_ResampleFilter::Pointer _pResizeFilter = T_ResampleFilter::New();
	_pResizeFilter->SetTransform(_pTransform);
	_pResizeFilter->SetInterpolator(_pInterpolator);

	const double vfOutputOrigin[1] = { 0.0 };
	_pResizeFilter->SetOutputOrigin(vfOutputOrigin);

	int nNewVelocity = round(nVelocity * timeInterval / newTimeInterval);
//	std::cout <<"SplineInterpolate-Debug: nNewVelocity="<< nNewVelocity << std::endl;

	double outSpacing[1];
	outSpacing[0] = newTimeInterval;
	_pResizeFilter->SetOutputSpacing(outSpacing);

	itk::Size<1> vnOutputSize =
	{	nNewVelocity};
	_pResizeFilter->SetSize(vnOutputSize);

	// Specify the input.
	_pResizeFilter->SetInput(importFilter->GetOutput());
	_pResizeFilter->Update();

	ImageType::Pointer imageAfterInterpolation = _pResizeFilter->GetOutput();

	ImageType::IndexType pixelIndex1D;
	for (pixelIndex1D[0] = 0; pixelIndex1D[0] < nNewVelocity; pixelIndex1D[0]++)
	{
		int index = pixelIndex1D[0];
		newVelocity[index] = imageAfterInterpolation->GetPixel(pixelIndex1D);
	}
}


void LineTraverseFit(int nList, float **pList, float point[2], // Input
		             float &slope, float &intercept) // Output
{
	float numerator = 0;
	float denominator = 0;
	for (int i = 0; i < nList; i++)
	{
		numerator = numerator + pList[i][0] * pList[i][1]
				- pList[i][0] * point[1];
		denominator = denominator + pList[i][0] * pList[i][0]
				- pList[i][0] * point[0];
	}

	if (denominator != 0)
		slope = numerator / denominator;
	else
		slope = 0;

	intercept = point[1] - slope * point[0];

}
